#ifndef PID_H_INCLUDED
#define PID_H_INCLUDED

class PID
{
public:
    PID()
    {
        m_iAcc = 0;
        m_last = 0;
    }

    void ResetIntegral()
    {
        m_iAcc = 0;
    }

    float Apply(const float command, const float actual)
    {

        float err = command - actual;
        float diff = (err - m_last);
        m_last = err;
        m_iAcc += err * i;
        if (fabs(m_iAcc) > iLim)
        {
            if (m_iAcc >0)
            {
                m_iAcc = iLim;
            }
            else
            {
                m_iAcc = -iLim;
            }
        }
        return ((err * p) + (diff * d) + m_iAcc);
//      return ((err * p) + (diff * d / period) + m_iAcc);
    }

    ParameterF p;
    ParameterF i;
    ParameterF d;
    ParameterF iLim;

private:
    float m_last;
    float m_iAcc;
};


#endif //#ifndef PID_H_INCLUDED

